﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace sconSDK
{
    public class Servo
    {
        public event EventHandler<ServoParams> PositionChanged;

        private int m_id;
        private Scon m_scon;

        public Servo(Scon scon, int id)
        {
            m_id = id;
            m_scon = scon;

            string values = m_scon.WriteAndRead("&$OV0R01002710xxxDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDxxxxxxxxxxxxxx0H~");
            values = values.Remove(0, 7);
            m_value = Convert.ToInt32(values.Substring((m_id - 1) * 4, 4), 16);
        }

        private int m_value;
        public int Value
        {
            get { return m_value; }
            set
            {
                m_value = value;
                string hexVal = "&-OV" + m_value.ToString("X4") + m_id.ToString("X2") + "H";
                string retVal = m_scon.WriteAndRead(hexVal);

                UpdateStatus();
            }
        }

        private void UpdateStatus()
        {
            if (PositionChanged != null)
            {
                PositionChanged.Invoke(this, new ServoParams(m_id, m_value));
            }
        }
    }
}
